<!DOCTYPE html>
<html lang="zh">
  <head>
    <meta charset="utf-8">
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <link rel="apple-touch-icon" href="apple-touch-icon.png">
    <link rel="shortcut icon" href="favicon.ico" />
    <link rel="manifest" href="a3.webmanifest">
    <title>A3 Template</title>
  </head>
  <body>
    <link rel="stylesheet" href="css/main.css"/>
    <script src="js/Theme.js"></script>
    <script src="js/ammo.js/build/ammo.wasm.js"></script>
    <div class="div-fullscreen"><div id="container"></div></div>
    <script type="module">
      import * as THREE from "./js/three.js/build/three.module.js";
      import Stats from './jsm/libs/stats.module.js';
		  import { OrbitControls } from './jsm/controls/OrbitControls.js';
      import { GUI } from './jsm/libs/dat.gui.module.js';
      import { Lut } from './jsm/math/Lut.js';
      import { ColladaLoader } from './jsm/loaders/ColladaLoader.js';
      import * as BufferGeometryUtils from './jsm/utils/BufferGeometryUtils.js';
      
      // Graphics variables
      let container, stats;
      let camera, controls, scene, renderer;
      let ambientLight;

      let textureLoader;
      const clock = new THREE.Clock();

      // Physics variables
      const gravityConstant = - 9.8;
      let collisionConfiguration;
      let dispatcher;
      let broadphase;
      let solver;
      let softBodySolver;
      let softBodyHelpers;
      let physicsWorld;
      const rigidBodies = [];
      const softBodies = [];
      const margin = 0.005;
      let transformAux1;

      // gui
      let gui;
      
      let params = {
        // 背景
        sceneBackGroundColor: 0x1982cc,
        sceneLight:0x000000,
        cameraFov:60,
        cameraFar:2000,
        cameraNear:0.2,
        // 仿真
        simSpeed:1,
        simSpeeds:[0.1, 0.2, 0.5, 1, 2, 5],
        simStart:true,
        simPause:false,
        simReset:false,
      }

      const lut = new Lut();

      lut.setColorMap( 'rainbow' );

      lut.setMax( 100 );
      lut.setMin( 0 );


      Ammo().then( function ( AmmoLib ) {

        Ammo = AmmoLib;

        init();
        animate();

      } );


      function init() {

        initGraphics();

        initPhysics();

        createObjects();

        initInput();

        setupGUI();

      }

      function reset(){

      }

      
      function initGraphics() {

        container = document.getElementById( 'container' );
        
        camera = new THREE.PerspectiveCamera(
          params.cameraFov, window.innerWidth / window.innerHeight, 
          params.cameraNear, params.cameraFar
        );

        scene = new THREE.Scene();
        scene.background = new THREE.Color( params.sceneBackGroundColor );

        const axesHelper = new THREE.AxesHelper();
        scene.add(axesHelper);

        camera.position.set( - 1, 50, 1 );

        renderer = new THREE.WebGLRenderer({ alpha: true });
        renderer.setClearColor(0XFFFFFF, 0);
        renderer.setPixelRatio( window.devicePixelRatio );
        renderer.setSize( window.innerWidth, window.innerHeight );
        renderer.shadowMap.enabled = true;
        container.appendChild( renderer.domElement );

        controls = new OrbitControls( camera, renderer.domElement );
        controls.target.set( 0, 0, 0 );
        controls.update();

        textureLoader = new THREE.TextureLoader();

        ambientLight = new THREE.AmbientLight( params.sceneLight );
        scene.add( ambientLight );

        const light = new THREE.DirectionalLight( 0xffffff, 1 );
        light.position.set( 0, 10, 0 );
        light.castShadow = true;
        const d = 10;
        light.shadow.camera.left = - d;
        light.shadow.camera.right = d;
        light.shadow.camera.top = d;
        light.shadow.camera.bottom = - d;

        light.shadow.camera.near = 2;
        light.shadow.camera.far = 50;

        light.shadow.mapSize.x = 1024;
        light.shadow.mapSize.y = 1024;

        scene.add( light );

        stats = new Stats();
        stats.domElement.style.position = 'absolute';
        stats.domElement.style.top = '0px';
        container.appendChild( stats.domElement );

        //

        // const sprite = new THREE.Sprite( new THREE.SpriteMaterial( {
				// 	map: new THREE.CanvasTexture( lut.createCanvas() )
				// } ) );
        // sprite.position.set(1, 1, 1);
				// sprite.scale.x = 0.25;
				// scene.add( sprite );

        window.addEventListener( 'resize', onWindowResize );

      }

      function initPhysics() {

        // Physics configuration

        collisionConfiguration = new Ammo.btSoftBodyRigidBodyCollisionConfiguration();
        dispatcher = new Ammo.btCollisionDispatcher( collisionConfiguration );
        broadphase = new Ammo.btDbvtBroadphase();
        solver = new Ammo.btSequentialImpulseConstraintSolver();
        softBodySolver = new Ammo.btDefaultSoftBodySolver();
        physicsWorld = new Ammo.btSoftRigidDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration, softBodySolver );
        physicsWorld.setGravity( new Ammo.btVector3( 0, gravityConstant, 0 ) );
        physicsWorld.getWorldInfo().set_m_gravity( new Ammo.btVector3( 0, gravityConstant, 0 ) );

        transformAux1 = new Ammo.btTransform();
        softBodyHelpers = new Ammo.btSoftBodyHelpers();

      }

      function createObjects() {
        const pos = new THREE.Vector3();
        const quat = new THREE.Quaternion();

        // Ground
        pos.set( 0, - 0.5, 0 );
        quat.set( 0, 0, 0, 1 );
        const ground = createParalellepiped( 40, 1, 40, 0, pos, quat, new THREE.MeshPhongMaterial( { color: 0xFFFFFF } ) );
        ground.castShadow = true;
        ground.receiveShadow = true;
        textureLoader.load( "textures/grid.png", function ( texture ) {

          texture.wrapS = THREE.RepeatWrapping;
          texture.wrapT = THREE.RepeatWrapping;
          texture.repeat.set( 40, 40 );
          ground.material.map = texture;
          ground.material.needsUpdate = true;

        } );

        // Tank
        const tankSize = 10;
        const tankThickness = 0.1;
        const tankMass = 0;
        
        const tankMaterial = new THREE.MeshPhongMaterial({ 
          color:0xffffff, 
          side:THREE.DoubleSide,
          transparent:true, 
          opacity: 0.2
        });
        pos.set((tankSize + tankThickness) * .5 , tankSize * .5, 0);
        const wall1 = createParalellepiped(tankThickness, tankSize, tankSize, tankMass, pos, quat, tankMaterial);
        pos.set(-(tankSize + tankThickness) * .5, tankSize * .5, 0);
        const wall2 = createParalellepiped(tankThickness, tankSize, tankSize, tankMass, pos, quat, tankMaterial);
        pos.set(0, tankSize * .5, (tankSize + tankThickness) * .5);
        const wall3 = createParalellepiped(tankSize + tankThickness * 2, tankSize, tankThickness, tankMass, pos, quat, tankMaterial);
        pos.set(0, tankSize * .5, -(tankSize + tankThickness) * .5);
        const wall4 = createParalellepiped(tankSize + tankThickness * 2, tankSize, tankThickness, tankMass, pos, quat, tankMaterial);

        // Balls
        const ballRadius = 1;
        const ballMass = 0.05;
        const count = tankSize / (ballRadius * 2);
        const layers = count * 0.5;
        // const ballMaterial = new THREE.MeshPhongMaterial({ color:0x000000 });
        for(var i = 0; i < layers ; i ++){
          for(var x = 0; x < count; x ++){
            for(var y = 0; y < count ; y ++){
              pos.set( 
                (x * 2 + 1) * ballRadius - tankSize / 2, 
                (i * 2 + 1) * ballRadius, 
                (y * 2 + 1) * ballRadius - tankSize / 2
              );
              const color = lut.getColor( i * 100 / layers);
              const ballMaterial = new THREE.MeshPhongMaterial({
                color:color,
                transparent: true,
                opacity:1
              });
              const ball = createSphered(ballRadius, ballMass, pos, quat, ballMaterial);
              // const sphereGeometry = new THREE.SphereGeometry( ballRadius, 40, 25 );
              // sphereGeometry.translate( pos.x, pos.y, pos.z );
				      // createSoftVolume( sphereGeometry, ballMass, 250 );
            }
          }
        }

        // 碰撞物体
        // const t1 = createParalellepiped(1,1,1,1, pos, quat);

        const manager = new THREE.LoadingManager();
        var path = './files/ur_description/meshes/ur5/collision/Wrist3.dae';
        const daeLoader = new ColladaLoader(manager);
        daeLoader.load(path, dae => {
          const mesh = dae.scene.children[0];

          // const mesh = new THREE.Mesh(new THREE.BoxGeometry(.1,.1,.1, 1, 1, 1));
          textureLoader.load( 'textures/colors.png', function ( texture ) {

            // texture.wrapS = THREE.RepeatWrapping;
            // texture.wrapT = THREE.RepeatWrapping;
            // texture.repeat.set( 1, 1 );
            mesh.material.map = texture;
            mesh.material.needsUpdate = true;

          } );
          // mesh.scale.set(10, 10, 10);
          console.log(mesh)
          // mesh.geometry.computeBoundingBox();
          // mesh.geometry.center();

          const axesHelper = new THREE.AxesHelper(0.5);
          mesh.add(axesHelper);
          const scale = 50;
          const positions = mesh.geometry.attributes.position.array;
          for(var i = 0, il = positions.length; i < il; i++){
            positions[i] *= scale;
          }

          // mesh.scale.set(scale, scale, scale)
          var shape = createConvexHullPhysicsShape(mesh.geometry.attributes.position.array, 1);
          
          // var shape = new Ammo.btBoxShape(new Ammo.btVector3(.05, 0.05, 0.05));
          pos.set(0, 20, 0);
          quat.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI/ 2);
          createRigidBody( mesh, shape, 10, pos, quat );
          const geometry = mesh.geometry.clone();
          console.log("Geom:" , geometry)
          
          // geometry.comupteBoundingBox();
          // geometry.center();
          // sphereGeometry.translate( 0, 20, 0 );
				  // createSoftVolume( geometry, 1, 250 );
        });

        // const volumeMass = 15;

				// const sphereGeometry = new THREE.SphereGeometry( 1, 40, 25 );
				// sphereGeometry.translate( 0, 20, 0 );
				// createSoftVolume( sphereGeometry, volumeMass, 250 );

      }


      function initInput() {

        window.addEventListener( 'keydown', function ( event ) {


        } );

        window.addEventListener( 'keyup', function () {

        } );

      }

      function setupGUI(){
        
        gui = new GUI();
        // 场景
        const guiScene = gui.addFolder('场景');
        guiScene.addColor(params, 'sceneBackGroundColor').name('背景色').onChange((val)=>{
          scene.background.set(val);
        });
        guiScene.addColor(params, 'sceneLight').name('环境光').onChange((val)=>{
          ambientLight.color.set(val);
        });
        guiScene.add(params, 'cameraFov', 1, 360, 1).name('相机视角').onChange((val)=>{
          camera.fov = val;
          camera.updateProjectionMatrix();
        });
        guiScene.add(params, 'cameraFar', 10, 9999, 1).name('相机远景').onChange((val)=>{
          camera.far = val;
          camera.updateProjectionMatrix();
        });
        guiScene.add(params, 'cameraNear', 0.01, 10, 0.01).name('相机近景').onChange((val)=>{
          camera.near = val;
          camera.updateProjectionMatrix();
        });

        // 仿真
        const guiSimulation = gui.addFolder('仿真');
        const guiSimStart = guiSimulation.add(params, 'simStart').name('开始').onChange((val)=>{
          if(val){
            guiSimStart.name('开始');
            guiSimReset.setValue(false);
          }
          else{
            guiSimStart.name('停止');
          }
        });
        const guiSimPause = guiSimulation.add(params, 'simPause').name('暂停').onChange((val)=>{
          if(!val){
            guiSimPause.name('暂停');
          }
          else{
            guiSimPause.name('继续');
          }
        });
        const guiSimReset = guiSimulation.add(params, 'simReset').name('重置').onChange((val)=>{
          if(val){
            reset();
            guiSimStart.setValue(true);
            guiSimPause.setValue(false);
          }
        });
        guiSimulation.add(params, 'simSpeed').name('仿真速度').options(params.simSpeeds);
      }

      function onWindowResize() {

        camera.aspect = window.innerWidth / window.innerHeight;
        camera.updateProjectionMatrix();

        renderer.setSize( window.innerWidth, window.innerHeight );

      }

      function processGeometry( bufGeometry ) {

        // Ony consider the position values when merging the vertices
        const posOnlyBufGeometry = new THREE.BufferGeometry();
        posOnlyBufGeometry.setAttribute( 'position', bufGeometry.getAttribute( 'position' ) );
        posOnlyBufGeometry.setIndex( bufGeometry.getIndex() );

        // Merge the vertices so the triangle soup is converted to indexed triangles
        const indexedBufferGeom = BufferGeometryUtils.mergeVertices( posOnlyBufGeometry );

        // Create index arrays mapping the indexed vertices to bufGeometry vertices
        mapIndices( bufGeometry, indexedBufferGeom );

      }

      function isEqual( x1, y1, z1, x2, y2, z2 ) {

        const delta = 0.000001;
        return Math.abs( x2 - x1 ) < delta &&
            Math.abs( y2 - y1 ) < delta &&
            Math.abs( z2 - z1 ) < delta;

      }

      function mapIndices( bufGeometry, indexedBufferGeom ) {

        // Creates ammoVertices, ammoIndices and ammoIndexAssociation in bufGeometry

        const vertices = bufGeometry.attributes.position.array;
        const idxVertices = indexedBufferGeom.attributes.position.array;
        const indices = indexedBufferGeom.index.array;

        const numIdxVertices = idxVertices.length / 3;
        const numVertices = vertices.length / 3;

        bufGeometry.ammoVertices = idxVertices;
        bufGeometry.ammoIndices = indices;
        bufGeometry.ammoIndexAssociation = [];

        for ( let i = 0; i < numIdxVertices; i ++ ) {

          const association = [];
          bufGeometry.ammoIndexAssociation.push( association );

          const i3 = i * 3;

          for ( let j = 0; j < numVertices; j ++ ) {

            const j3 = j * 3;
            if ( isEqual( idxVertices[ i3 ], idxVertices[ i3 + 1 ], idxVertices[ i3 + 2 ],
              vertices[ j3 ], vertices[ j3 + 1 ], vertices[ j3 + 2 ] ) ) {

              association.push( j3 );

            }

          }

        }

      }

      function createSoftVolume( bufferGeom, mass, pressure ) {

        processGeometry( bufferGeom );

        console.log(bufferGeom)

        const volume = new THREE.Mesh( bufferGeom, new THREE.MeshPhongMaterial( { color: 0xFFFFFF } ) );
        volume.castShadow = true;
        volume.receiveShadow = true;
        volume.frustumCulled = false;
        scene.add( volume );

        textureLoader.load( 'textures/colors.png', function ( texture ) {

          volume.material.map = texture;
          volume.material.needsUpdate = true;

        } );

        // Volume physic object

        const volumeSoftBody = softBodyHelpers.CreateFromTriMesh(
          physicsWorld.getWorldInfo(),
          bufferGeom.ammoVertices,
          bufferGeom.ammoIndices,
          bufferGeom.ammoIndices.length / 3,
          true );

        const sbConfig = volumeSoftBody.get_m_cfg();
        sbConfig.set_viterations( 40 );
        sbConfig.set_piterations( 40 );

        // Soft-soft and soft-rigid collisions
        sbConfig.set_collisions( 0x11 );

        // Friction
        sbConfig.set_kDF( 0.1 );
        // Damping
        sbConfig.set_kDP( 0.01 );
        // Pressure
        sbConfig.set_kPR( pressure );
        // Stiffness
        volumeSoftBody.get_m_materials().at( 0 ).set_m_kLST( 0.9 );
        volumeSoftBody.get_m_materials().at( 0 ).set_m_kAST( 0.9 );

        volumeSoftBody.setTotalMass( mass, false );
        Ammo.castObject( volumeSoftBody, Ammo.btCollisionObject ).getCollisionShape().setMargin( margin );
        physicsWorld.addSoftBody( volumeSoftBody, 1, - 1 );
        volume.userData.physicsBody = volumeSoftBody;
        // Disable deactivation
        volumeSoftBody.setActivationState( 4 );

        softBodies.push( volume );

      }


      function createConvexHullPhysicsShape( coords , scale = 10) {

        const shape = new Ammo.btConvexHullShape();
        var tempBtVec3_1 = new Ammo.btVector3( 0, 0, 0 );
        for ( let i = 0, il = coords.length; i < il; i += 3 ) {

          tempBtVec3_1.setValue( coords[ i ] * scale, coords[ i + 1 ] * scale, coords[ i + 2 ]  * scale);
          const lastOne = ( i >= ( il - 3 ) );
          shape.addPoint( tempBtVec3_1, lastOne );

        }
        shape.setLocalScaling(new Ammo.btVector3( .5, .5, .5 ));
        // console.log(shape)
        shape.setMargin(margin);
        return shape;

      }
      function createParalellepiped( sx, sy, sz, mass, pos, quat, material ) {

      const threeObject = new THREE.Mesh( new THREE.BoxGeometry( sx, sy, sz, 1, 1, 1 ), material );
      const shape = new Ammo.btBoxShape( new Ammo.btVector3( sx * 0.5, sy * 0.5, sz * 0.5 ) );
      shape.setMargin( margin );

      createRigidBody( threeObject, shape, mass, pos, quat );

      return threeObject;

      }

      function createSphered( r, mass, pos, quat, material) {

        const threeObject = new THREE.Mesh( new THREE.SphereGeometry( r, 24, 24 ), material );
        const shape = new Ammo.btSphereShape( r );
        shape.setMargin( margin );

        createRigidBody( threeObject, shape, mass, pos, quat);

        return threeObject;

      }

      function createRigidBody( threeObject, physicsShape, mass, pos, quat ) {

        threeObject.position.copy( pos );
        threeObject.quaternion.copy( quat );

        const transform = new Ammo.btTransform();
        transform.setIdentity();
        transform.setOrigin( new Ammo.btVector3( pos.x, pos.y, pos.z ) );
        transform.setRotation( new Ammo.btQuaternion( quat.x, quat.y, quat.z, quat.w ) );
        const motionState = new Ammo.btDefaultMotionState( transform );

        const localInertia = new Ammo.btVector3( 0, 0, 0 );
        physicsShape.calculateLocalInertia( mass, localInertia );

        const rbInfo = new Ammo.btRigidBodyConstructionInfo( mass, motionState, physicsShape, localInertia );
        const body = new Ammo.btRigidBody( rbInfo );

        threeObject.userData.physicsBody = body;

        scene.add( threeObject );

        if ( mass > 0 ) {

          rigidBodies.push( threeObject );

          // Disable deactivation
          body.setActivationState( 4 );

        }

        physicsWorld.addRigidBody( body );

        }

      function animate() {

        requestAnimationFrame( animate );

        render();
        stats.update();

      }

      function render() {

        const deltaTime = clock.getDelta() / params.simSpeed;
        updatePhysics( deltaTime )
        
        renderer.setViewport( 0, 0,  window.innerWidth, window.innerHeight );
        renderer.render( scene, camera ); 
      }

      function updatePhysics( deltaTime ) {

        // Step world
        physicsWorld.stepSimulation( deltaTime );

        // Update soft volumes
				for ( let i = 0, il = softBodies.length; i < il; i ++ ) {

          const volume = softBodies[ i ];
          const geometry = volume.geometry;
          const softBody = volume.userData.physicsBody;
          const volumePositions = geometry.attributes.position.array;
          const volumeNormals = geometry.attributes.normal.array;
          const association = geometry.ammoIndexAssociation;
          const numVerts = association.length;
          const nodes = softBody.get_m_nodes();
          for ( let j = 0; j < numVerts; j ++ ) {

            const node = nodes.at( j );
            const nodePos = node.get_m_x();
            const x = nodePos.x();
            const y = nodePos.y();
            const z = nodePos.z();
            const nodeNormal = node.get_m_n();
            const nx = nodeNormal.x();
            const ny = nodeNormal.y();
            const nz = nodeNormal.z();

            const assocVertex = association[ j ];

            for ( let k = 0, kl = assocVertex.length; k < kl; k ++ ) {

              let indexVertex = assocVertex[ k ];
              volumePositions[ indexVertex ] = x;
              volumeNormals[ indexVertex ] = nx;
              indexVertex ++;
              volumePositions[ indexVertex ] = y;
              volumeNormals[ indexVertex ] = ny;
              indexVertex ++;
              volumePositions[ indexVertex ] = z;
              volumeNormals[ indexVertex ] = nz;

            }

          }

          geometry.attributes.position.needsUpdate = true;
          geometry.attributes.normal.needsUpdate = true;

         }

        // Update rigid bodies
        for ( let i = 0, il = rigidBodies.length; i < il; i ++ ) {

          const objThree = rigidBodies[ i ];
          const objPhys = objThree.userData.physicsBody;
          const ms = objPhys.getMotionState();
          if ( ms ) {

            ms.getWorldTransform( transformAux1 );
            const p = transformAux1.getOrigin();
            const q = transformAux1.getRotation();
            objThree.position.set( p.x(), p.y(), p.z() );
            objThree.quaternion.set( q.x(), q.y(), q.z(), q.w() );

          }
        }
        
      }
      
    </script>
  </body>
</html>